#include <graphics/Skeleton.h>
#include <graphics/SkeletonFileFormat.h>
#include <system/Log.h>

namespace ne
{
    const uint16_t Skeleton::MAX_JOINT_NUM_IN_ONE_SKELETON = 0xFFFF;

    Skeleton::Skeleton(ResourceManager *pManager,
                       const uint64_t id,
                       const std::string &name,
                       const std::string &fileName,
                       const bool isManual,
                       ManualResourceLoader *pLoader)
        : Resource(pManager, id, name, fileName, isManual, pLoader),
          mManualJointsDirty(false)
    {
        for (uint16_t handle=0; handle<=Joint::MAX_JOINT_HANDLE; ++handle)
        {
            mAutoNextHandleSet.insert(handle);
        }
    }

    Joint* Skeleton::createJoint()
    {
        // Make a handle
        if (!mAutoNextHandleSet.empty())
        {
            uint16_t handle = *mAutoNextHandleSet.begin();
            return createJoint(handle);
        }
        return 0;
    }
            
    Joint* Skeleton::createJoint(const uint16_t handle)
    {
        std::string name = "Joint";
        std::stringstream num;
        num << handle;
        name += num.str();
        return createJoint(handle, name);
    }

    Joint* Skeleton::createJoint(const std::string &name)
    {
        // Make a handle
        if (!mAutoNextHandleSet.empty())
        {
            uint16_t handle = *mAutoNextHandleSet.begin();
            return createJoint(handle, name);
        }
        return 0;
    }

    Joint* Skeleton::createJoint(const uint16_t handle, const std::string &name)
    {
        // Check handle can use
        std::set<uint16_t>::iterator it = mAutoNextHandleSet.find(handle);
        if (it != mAutoNextHandleSet.end())
        {
            // Check name can use
            JointNameMapIterator nameIt = mJointNameSearchMap.find(name);
            if (nameIt == mJointNameSearchMap.end())
            {
                // Remove the handle in AutoNextHandleSet
                mAutoNextHandleSet.erase(handle);
                Joint *joint = new Joint(this, handle, name);
                mJoints.push_back(joint);
                mJointHandleSearchMap[handle] = joint;
                mJointNameSearchMap[name] = joint;
                return joint;
            }
        }
        return 0;
    }

    Joint* Skeleton::getJoint(const uint16_t handle) const
    {
        JointHandleMapConstIterator it = mJointHandleSearchMap.find(handle);
        if (it != mJointHandleSearchMap.end())
        {
            return it->second;
        }
        return 0;
    }

    Joint* Skeleton::getJoint(const std::string &name) const
    {
        JointNameMapConstIterator it = mJointNameSearchMap.find(name);
        if (it != mJointNameSearchMap.end())
        {
            return it->second;
        }
        return 0;
    }

    bool Skeleton::destroyJoint(Joint *joint)
    {
        if (joint)
        {
            JointVectorIterator it = std::find(mJoints.begin(), mJoints.end(), joint);
            if (it != mJoints.end())
            {
                // Remove from HandleMap
                uint16_t handle = joint->getHandle();
                mJointHandleSearchMap.erase(handle);

                // Remove from NameMap
                std::string name = joint->getObjectName();
                mJointNameSearchMap.erase(name);

                // Remove from RootVector
                JointVectorIterator pos = std::find(mRootJoints.begin(), mRootJoints.end(), joint);
                if (pos != mRootJoints.end())
                {
                    mRootJoints.erase(pos);
                }

                // Remove from ManualSet
                mManualJoints.erase(joint);

                // Destroy from JointVector
                delete joint;
                mJoints.erase(it);

                // Insert the auto handle
                mAutoNextHandleSet.insert(handle);

                return true;
            }
        }
        return false;
    }

    bool Skeleton::destroyJoint(const uint16_t handle)
    {
        JointHandleMapIterator it = mJointHandleSearchMap.find(handle);
        return destroyJoint(it->second);
    }

    bool Skeleton::destroyJoint(const std::string &name)
    {
        JointNameMapIterator it = mJointNameSearchMap.find(name);
        return destroyJoint(it->second);
    }

    void Skeleton::destroyAllJoint()
    {
        for (JointVectorIterator it=mJoints.begin(); it!=mJoints.end(); ++it)
        {
            delete (*it);
        }
        mJoints.clear();
        mJointHandleSearchMap.clear();
        mJointNameSearchMap.clear();
        mRootJoints.clear();
        mManualJoints.clear();
        mAutoNextHandleSet.clear();
        for (uint16_t handle=0; handle<=Joint::MAX_JOINT_HANDLE; ++handle)
        {
            mAutoNextHandleSet.insert(handle);
        }
        mManualJointsDirty = false;
    }

    size_t Skeleton::getNumJoints() const
    {
        return mJointHandleSearchMap.size();
    }

    bool Skeleton::hasJoint(const uint16_t handle) const
    {
        JointHandleMapConstIterator it = mJointHandleSearchMap.find(handle);
        if (it != mJointHandleSearchMap.end())
        {
            return true;
        }
        return false;
    }

    bool Skeleton::hasJoint(const std::string &name) const
    {
        JointNameMapConstIterator it = mJointNameSearchMap.find(name);
        if (it != mJointNameSearchMap.end())
        {
            return true;
        }
        return false;
    }

    bool Skeleton::isInternalJoint(const Joint *joint) const
    {
        if (joint)
        {
            JointHandleMapConstIterator it = mJointHandleSearchMap.find(joint->getHandle());
            if (it != mJointHandleSearchMap.end() && it->second == joint)
            {
                return true;
            }
        }
        return false;
    }

    void Skeleton::setBinding()
    {
        _update();

        std::sort(mJoints.begin(), mJoints.end(), JointHandleLess);

        for (JointVectorIterator it=mJoints.begin(); it!=mJoints.end(); ++it)
        {
            (*it)->setBinding();
        }
    }

    void Skeleton::deriveRootJoint()
    {
        mRootJoints.clear();
        for (JointVectorConstIterator it=mJoints.begin(); it!=mJoints.end(); ++it)
        {
            if ((*it)->getParent() == 0)
            {
                mRootJoints.push_back(*it);
            }
        }
    }

    void Skeleton::reset(const bool resetManualJoints)
    {
        for (JointVectorIterator it=mJoints.begin(); it!=mJoints.end(); ++it)
        {
            if (!(*it)->isManuallyControlled() || resetManualJoints)
            {
                (*it)->reset();
            }
        }
    }

    // FIXME: Need to fix
    void Skeleton::update()
    {
        _update();
    }

    bool Skeleton::isManualJointsDirty() const
    {
        return mManualJointsDirty;
    }

    bool Skeleton::hasManualJoints() const
    {
        if (mManualJoints.empty())
        {
            return false;
        }
        return true;
    }

    void Skeleton::getAllJointsOffsetTransformMatrices(Matrix4Vector &matrices)
    {
        _update();

        matrices.clear();
        matrices.reserve(mJoints.size());
        matrices.resize(mJoints.size());

        JointVectorConstIterator it = mJoints.begin();
        for (size_t i=0; it!=mJoints.end(); ++i, ++it)
        {
            (*it)->getOffsetTransformMatrix(matrices[i]);
        }
    }

    const JointVector& Skeleton::getJointVector() const
    {
        return mJoints;
    }

    void Skeleton::copyFrom(const Skeleton &skeleton)
    {
        destroyAllJoint();

        const JointVector &joints = skeleton.getJointVector();
        size_t numJoints = joints.size();
        std::vector<uint16_t> handleList(numJoints);
        std::vector<uint16_t> parentList(numJoints);
        for (size_t i=0; i<numJoints; ++i)
        {
            Joint *pJoint = createJoint(joints[i]->getHandle(), joints[i]->getObjectName());
            if (pJoint)
            {
                handleList[i] = joints[i]->getHandle();
                Joint *pParent = dynamic_cast<Joint*>(joints[i]->getParent());
                if (pParent)
                {
                    parentList[i] = pParent->getHandle();
                }
                else
                {
                    parentList[i] = Joint::JOINT_NO_PARENT_HANDLE;
                }
                pJoint->setPosition(joints[i]->getPosition(TS_PARENT));
                pJoint->setOrientation(joints[i]->getOrientation(TS_PARENT));
                pJoint->setScale(joints[i]->getScale(TS_PARENT));
            }
        }

        for (size_t i=0; i<numJoints; ++i)
        {
            Joint *pJoint = getJoint(handleList[i]);
            Joint *pParent = getJoint(parentList[i]);
            if (pJoint && pParent)
            {
                pParent->addChild(pJoint);
            }
        }

        setFileName(skeleton.getFileName());

        setBinding();
        deriveRootJoint();
    }

    Skeleton::~Skeleton()
    {
        unload();
    }

    // Change the name for JointNameSearchMap
    bool Skeleton::_prepareRenameJoint(Joint *joint, const std::string &name)
    {
        if (isInternalJoint(joint))
        {
            if (!getJoint(name))
            {
                mJointNameSearchMap.erase(joint->getObjectName());
                mJointNameSearchMap[name] = joint;
                return true;
            }
        }
        return false;
    }

    void Skeleton::_notifyManualJointsDirty()
    {
        mManualJointsDirty = true;
    }

    void Skeleton::_notifyManualJointChange(Joint *joint)
    {
        if (isInternalJoint(joint))
        {
            if (joint->isManuallyControlled())
            {
                mManualJoints.insert(joint);
            }
            else
            {
                mManualJoints.erase(joint);
            }
        }
    }

    bool Skeleton::loadImplement()
    {
        FILE *file = openFile("rb");
        if (!file)
        {
            Log::Out(LL_WARNING,
                "Open Skeleton (%s) file (%s) failed", getName().c_str(), getFileName().c_str());
            return false;
        }

        SkeletonHeader skeletonHeader;
        if (!fread(&skeletonHeader, sizeof(SkeletonHeader), 1, file))
        {
            closeFile(file);
            Log::Out(LL_WARNING,
                "Load Skeleton (%s) from file (%s) failed", getName().c_str(), getFileName().c_str());
            return false;
        }

        if (skeletonHeader.mMagicNum != SKELETON_FILE_MAGIC_NUMBER ||
            skeletonHeader.mVersion != SKELETON_FILE_VERSION)
        {
            closeFile(file);
            Log::Out(LL_WARNING,
                "Load Skeleton (%s) from file (%s) failed with wrong Magic or Version", getName().c_str(), getFileName().c_str());
            return false;
        }

        // Seek to JointHeader
        fseek(file, skeletonHeader.mJointsOffset, SEEK_SET);

        // Create Joint
        JointHeader jointHeaders[skeletonHeader.mNumJoints];
        if (!fread(jointHeaders, sizeof(JointHeader), skeletonHeader.mNumJoints, file))
        {
            closeFile(file);
            Log::Out(LL_WARNING,
                "Load Joint for Skeleton (%s) from file (%s) failed", getName().c_str(), getFileName().c_str());
            return false;
        }

        closeFile(file);

        std::vector<uint16_t> handleList(skeletonHeader.mNumJoints);
        std::vector<uint16_t> parentList(skeletonHeader.mNumJoints);

        for (uint32_t i=0; i<skeletonHeader.mNumJoints; ++i)
        {
            Joint *pJoint = createJoint(jointHeaders[i].mHandle, jointHeaders[i].mName);
            if (pJoint)
            {
                handleList[i] = jointHeaders[i].mHandle;
                parentList[i] = jointHeaders[i].mParent;
                pJoint->setPosition(Vector3(jointHeaders[i].mPosition));
                pJoint->setOrientation(Quaternion(jointHeaders[i].mOrientation));
                pJoint->setScale(Vector3(jointHeaders[i].mScale));
            }
            else
            {
                Log::Out(LL_WARNING,
                    "Create Joint for Skeleton (%s) from file (%s) failed", getName().c_str(), getFileName().c_str());
                return false;
            }
        }

        for (uint32_t i=0; i<skeletonHeader.mNumJoints; ++i)
        {
            Joint *pJoint = getJoint(handleList[i]);
            Joint *pParent = getJoint(parentList[i]);
            if (pJoint && pParent)
            {
                pParent->addChild(pJoint);
            }
        }

        setBinding();
        deriveRootJoint();

        return true;
    }

    bool Skeleton::unloadImplement()
    {
        destroyAllJoint();
        return true;
    }

    bool Skeleton::downloadImplement()
    {
        FILE *file = openFile("wb");
        if (!file)
        {
            Log::Out(LL_WARNING,
                "Create Skeleton (%s) file (%s) failed", getName().c_str(), getFileName().c_str());
            return false;
        }

        SkeletonHeader skeletonHeader;
        memset(&skeletonHeader, 0, sizeof(SkeletonHeader));

        skeletonHeader.mMagicNum = SKELETON_FILE_MAGIC_NUMBER;
        skeletonHeader.mVersion = SKELETON_FILE_VERSION;
        strncpy(skeletonHeader.mName, mName.c_str(),
                MAX_STRING_LENGTH_IN_SKELETON_FILE);

        skeletonHeader.mNumJoints = getNumJoints();
        fwrite(&skeletonHeader, sizeof(SkeletonHeader), 1, file);
        skeletonHeader.mJointsOffset = ftell(file);
        fseek(file, 0, SEEK_SET);
        fwrite(&skeletonHeader, sizeof(SkeletonHeader), 1, file);

        JointHeader jointHeaders[skeletonHeader.mNumJoints];
        memset(jointHeaders, 0, sizeof(JointHeader) * skeletonHeader.mNumJoints);

        for (uint32_t i=0; i<skeletonHeader.mNumJoints; ++i)
        {
            Joint *pJoint = mJoints[i];
            if (pJoint)
            {
                jointHeaders[i].mHandle = pJoint->getHandle();
                Joint *pParent = dynamic_cast<Joint*>(pJoint->getParent());
                if (pParent)
                {
                    jointHeaders[i].mParent = pParent->getHandle();
                }
                else
                {
                    jointHeaders[i].mParent = Joint::JOINT_NO_PARENT_HANDLE;
                }
                
                strncpy(jointHeaders[i].mName, pJoint->getObjectName().c_str(),
                        MAX_STRING_LENGTH_IN_SKELETON_FILE);

                Vector3 position = pJoint->getInitialPosition();
                jointHeaders[i].mPosition[0] = position.x;
                jointHeaders[i].mPosition[1] = position.y;
                jointHeaders[i].mPosition[2] = position.z;

                Quaternion orientation = pJoint->getInitialOrientation();
                jointHeaders[i].mOrientation[0] = orientation.x;
                jointHeaders[i].mOrientation[1] = orientation.y;
                jointHeaders[i].mOrientation[2] = orientation.z;
                jointHeaders[i].mOrientation[3] = orientation.w;

                Vector3 scale = pJoint->getInitialScale();
                jointHeaders[i].mScale[0] = scale.x;
                jointHeaders[i].mScale[1] = scale.y;
                jointHeaders[i].mScale[2] = scale.z;
            }
        }

        fseek(file, skeletonHeader.mJointsOffset, SEEK_SET);
        fwrite(jointHeaders, sizeof(JointHeader), skeletonHeader.mNumJoints, file);
        closeFile(file);

        return true;
    }

    size_t Skeleton::_calculateSize() const
    {
        return mSize;
    }

    void Skeleton::_update()
    {
        for (JointVectorIterator it=mRootJoints.begin(); it!=mRootJoints.end(); ++it)
        {
            (*it)->update();
        }
        mManualJointsDirty = false;
    }

    bool Skeleton::JointHandleLess(const Joint *jointA, const Joint *jointB)
    {
        if (jointA && jointB)
        {
            return jointA->getHandle() < jointB->getHandle();
        }
        if (jointB)
        {
            return false;
        }
        return true;
    }
}
